#include "device.h"
#include "modbus.h"
#include "math.h"
#include "speed_cnrt.h"
#include "astar.h"
// 添加 stdlib.h 头文件以支持 labs 函数
#include <stdlib.h>

// M电机参数定义
#define M_STEPS_PER_REVOLUTION  400     // M电机每转步数 (对应SPR)
#define SPEED_TO_RPS_FACTOR     628.318 // speed(0.01 rad/s) 转 rps 的系数 (100 × 2π)

// 基于freq.pl算法的时间转步数计算函数
// time_seconds: 转动时间（秒）
// speed: 转速（0.01 rad/s单位，与freq.pl中定义一致）
// 返回: 对应的步数
long Calculate_Steps_From_Time_FreqPL(int time_seconds, int speed)
{
    // speed单位是0.01 rad/s，转换为转/秒(rps)
    float rps = (float)speed / SPEED_TO_RPS_FACTOR;

    // 计算总步数：转/秒 × 时间 × 每转步数
    long steps = (long)(rps * time_seconds * M_STEPS_PER_REVOLUTION);
    
    return steps;
}

// 从x_target高16位提取时间的辅助函数
int Extract_Time_From_XTarget(long x_target)
{
    return (int)(x_target >> 16);  // 高16位作为时间（秒）
}

#define PRODUCT_DEBUG 1

#ifndef IGNORE_Z
#define MAX_SLAVE_COUNT         1
#else
#define MAX_SLAVE_COUNT         0
#endif
#define SLAVE_ADDRESS(n)        (1 + (n))

bit pending_action = 0;         // 新动作

// M电机霍尔传感器控制
bit hall_detection_enabled = 0; // M电机霍尔传感器检测使能标志

// Z电机运行参数
bit z_pick_cup_state_changed;   // 抓杯传感器状态改变
bit holding_cup_fallen;         // 从有测试杯到无测试杯触发
bit holding_cup_risen;          // 从无测试杯到有测试杯触发
bit check_holding;              // 检测测试杯有无触发沿
bit pick_cup_fallen;            // 抓杯传感器下降沿
bit pick_cup_risen;             // 抓杯传感器上升沿，从不遮挡到遮挡
long z_buffer_step;             // 检测到抓杯到位传感器后继续前进位置

bit filter_level                = 0;        // x传感器滤波电平
uchar level                     = 0;        // x传感器电平
uchar x_status                  = 0;        // x电机状态
bit open_triggered              = 0;        // x打开抓手动作已经触发过
long x_pos                      = 0;        // x电机当前坐标
ushort x_duration               = 0;        // x运动经过的时长
ulong x_distance                = 0;        // x经过的位移
////////////////////////////////////////////////////////////////////////////////

void Update_IO()
{
    ro_bit.x_at_home = X_AT_HOME;
    ro_bit.y_at_home = Y_AT_HOME;
    ro_bit.z_at_home = Z_AT_HOME;

    if (ro_bit.pick_cup_sensor != IO_PICK_CUP)
    {
        ro_bit.pick_cup_sensor = !ro_bit.pick_cup_sensor;
        if (ro_bit.pick_cup_sensor)
            pick_cup_risen = 1;
        else
            pick_cup_fallen = 1;
    }

    if (ro_bit.holding_cup != IO_HOLDING_CUP)
    {
        ro_bit.holding_cup = !ro_bit.holding_cup;
        if (ro_bit.holding_cup)
            holding_cup_risen = 1;
        else
            holding_cup_fallen = 1;
    }
}

////////////////////////////////////////////////////////////////////////////////
// X电机指令(X 电机用于驱动抓手打开/关闭)

// X电机更新定时器,频率输出
void X_Set_Timer(ushort tick)
{
    tick = tick > MAX_C ? MAX_C : tick;
    TIMER_CAR(TIMER3) = tick*2;
    TIMER_CH2CV(TIMER3) = tick;
}

// X电机停止定时器
void X_Stop_Timer()
{
    timer_disable(TIMER3);
}

// 启动X电机
void Start_X_Motor()
{
    ro_word.x_distance = 0;

    IO_X_DIR(X_DIR_OUTPUT(srd_x.dir));
    IO_X_EN(1);
    IO_X_LOCK(0);
    X_Set_Timer(FREQ_TABLE(srd_x.speed_index));
    timer_enable(TIMER3);
}

// 停止X电机
void Stop_X_Motor()
{
    IO_X_LOCK(1);  // 锁紧电机
    if (srd_x.dir == CCW)
        ro_word.x_distance = -srd_x.step_count;
    else
        ro_word.x_distance = srd_x.step_count;
    ro_word.x_duration = (SystemTick() - srd_x.start_time)/1000;
    srd_x.need_cleanup = 0;
    srd_x.running = 0;
    open_triggered = 0;
}

// 执行复位指令时, 每一步调用一次
void X_Reset_Update()
{
#if X_HOME_FILTER_ENABLE
    level = (level << 1) | X_AT_HOME;
    if((level == 0xff) || level == 0)
        filter_level = (level != 0);
#else
    filter_level = X_AT_HOME;
#endif

    // 判定原点状态改变
    if (filter_level != srd_x.home_state)
    {
        srd_x.home_state = !srd_x.home_state;
        srd_x.hit_home = 1;
        // 如果电机从非零点位置复位，当前 x_pos 在零点矫正范围内, 才矫正零点
        if(srd_x.home_state)
        {
            if(ro_word.x_pos < X_HOME_MIN_ERR_STEP)
            {
                srd_x.step_left = X_RESET_BUFFER_STEP;
                ro_word.x_pos = 0;
            }
            else
            {
                // 可能发生了信号干扰,也可能电机发生了丢步
                srd_x.step_left = X_PICKER_OPENED_POS;
            }
        }
    }
    else
        ro_word.x_pos += srd_x.step_offset;
}

// 复位指令停止时调用
void X_Reset_Cleanup()
{
    x_status = srd_x.hit_home ? STATUS_IDLE : X_STATUS_M_HOME_SENSOR_ERROR;

    Stop_X_Motor();
}

// 执行移动指令时, 每一步调用一次
void X_Move_To_Update()
{
#if X_HOME_FILTER_ENABLE
    level = (level << 1) | X_AT_HOME;
    if((level == 0xff) || level == 0)
        filter_level = (level != 0);
#else
    filter_level = X_AT_HOME;
#endif

    // 判定原点状态改变
    if (filter_level != srd_x.home_state)
    {
        srd_x.home_state = !srd_x.home_state;
        srd_x.hit_home = 1;
        ro_word.x_pos = 0;
        srd_x.step_left = labs(srd_x.target);
    }
    else
        ro_word.x_pos += srd_x.step_offset;
}

// 移动指令停止时调用
void X_Move_To_Cleanup()
{
    x_status = srd_x.hit_home ? STATUS_IDLE : X_STATUS_M_HOME_SENSOR_ERROR;
    x_status = (X_AT_HOME && ro_word.x_pos >= X_PICKER_OPENED_POS) ? X_STATUS_M_HOME_SENSOR_ERROR : STATUS_IDLE;

    Stop_X_Motor();
}

void X_Init_Reset()
{
    srd_x.init = Start_X_Motor;
    srd_x.start_timer = X_Set_Timer;
    srd_x.stop_timer = X_Stop_Timer;
    srd_x.update = X_Reset_Update;
    srd_x.cleanup = X_Reset_Cleanup;
    srd_x.home_state = X_AT_HOME;
    filter_level = srd_x.home_state;
    level = srd_x.home_state ? 0xff : 0;
    srd_x.coder_self = 1;
    srd_x.micro = 16;
}

void X_Init_Move(bit ignore_step_left)
{
    srd_x.init = Start_X_Motor;
    srd_x.start_timer = X_Set_Timer;
    srd_x.stop_timer = X_Stop_Timer;
    srd_x.update = X_Move_To_Update;
    srd_x.cleanup = X_Move_To_Cleanup;
    srd_x.home_state = X_AT_HOME;
    filter_level = srd_x.home_state;
    level = srd_x.home_state ? 0xff : 0;
    srd_x.ignore_step_left = ignore_step_left;
    open_triggered = 0;
    srd_x.coder_self = 1;
    srd_x.micro = 16;
}

void Do_Action_Open_Picker_No_Wait()
{
    long step = 0;
    step = (X_PICKER_OPENED_POS - ro_word.x_pos) > X_MAX_DISTANCE_STEP ? X_MAX_DISTANCE_STEP : (X_PICKER_OPENED_POS - ro_word.x_pos);

    X_Init_Move(0);
    srd_x.target = X_PICKER_OPENED_POS;
    speed_cntr_Move(&srd_x, step, X_MAX_SPEED);
    open_triggered = 1;
}

void Do_Action_Open_Picker( )
{
    long step = 0;
    if(open_triggered)
    {
        while(srd_x.running)
            Process_Main_Loop();
        return;
    }
    step = (X_PICKER_OPENED_POS - ro_word.x_pos) > X_MAX_DISTANCE_STEP ? X_MAX_DISTANCE_STEP : (X_PICKER_OPENED_POS - ro_word.x_pos);

    X_Init_Move(0);
    srd_x.target = X_PICKER_OPENED_POS;
    speed_cntr_Move(&srd_x, step, X_MAX_SPEED);
    while(srd_x.running)
        Process_Main_Loop();
}

void Do_Action_Close_Picker()
{
    // 在复位位置和X复位传感器未失效,不动作
    if(X_AT_HOME && x_status != X_STATUS_M_HOME_SENSOR_ERROR)
        return;

    X_Init_Reset();
    srd_x.target = -X_RESET_BUFFER_STEP;

    speed_cntr_Move(&srd_x, -X_MAX_DISTANCE_STEP, X_MAX_SPEED);

    // 等待X完成
    while(srd_x.running)
        Process_Main_Loop();
}

////////////////////////////////////////////////////////////////////////////////
// Z电机指令

// Z电机更新定时器
void Z_Set_Timer(ushort tick)
{
    tick = tick > MAX_C ? MAX_C : tick;
    TIMER_CAR(TIMER2) = tick*2;
    TIMER_CH0CV(TIMER2) = tick;
}

// Z电机停止定时器
void Z_Stop_Timer()
{
    timer_disable(TIMER2);
}

// 启动z电机
void Start_Z_Motor()
{
    ro_word.z_distance = 0;

    IO_Z_DIR(srd_z.dir);
    IO_Z_EN(1);
    IO_Z_LOCK(0);

    pick_cup_risen = 0;
    pick_cup_fallen = 0;
    holding_cup_risen = 0;
    holding_cup_fallen = 0;

    Z_Set_Timer(FREQ_TABLE(srd_z.speed_index));
    timer_enable(TIMER2);
}

// 停止Z电机
void Stop_Z_Motor()
{
    IO_Z_LOCK(1);  // 锁紧电机
    if (srd_z.dir == CCW)
        ro_word.z_distance = -srd_z.step_count;
    else
        ro_word.z_distance = srd_z.step_count;
    ro_word.z_duration = (SystemTick() - srd_z.start_time)/1000;
    srd_z.need_cleanup = 0;
    srd_z.running = 0;
}

// 执行复位指令时, 每一步调用一次
void Z_Reset_Update()
{
    // 判定原点状态改变
    if (Z_AT_HOME != srd_z.home_state)
    {
        srd_z.home_state = !srd_z.home_state;
        srd_z.hit_home = 1;
        ro_word.z_pos = 0;
        srd_z.step_left = Z_RESET_BUFFER_STEP;
    }
    else
        ro_word.z_pos += srd_z.step_offset;
}

// 复位指令结束时调用
void Z_Reset_Cleanup()
{
    if (!srd_z.hit_home)
        server_status = XY_STATUS_Z_HOME_SENSOR_ERROR;

    Stop_Z_Motor();
}

// 执行移动指令时, 每一步调用一次
void Z_Move_To_Update()
{
    // 判定原点状态改变
    if (Z_AT_HOME != srd_z.home_state)
    {
        srd_z.home_state = !srd_z.home_state;
        srd_z.hit_home = 1;
        ro_word.z_pos = 0;
        if (!srd_z.ignore_step_left)
        {
            srd_z.step_left = labs(srd_z.target);
        }
    }
    else
        ro_word.z_pos += srd_z.step_offset;
}

// 移动指令结束时调用
void Z_Move_To_Cleanup()
{
    Stop_Z_Motor();
}

// 探测测试杯位置时, 每一步调用一次
void Z_Pick_Cup_Update()
{
    Update_IO();
    Z_Move_To_Update();

#if USE_PICK_CUP_SENSOR
    // 判定机械手抓杯传感器状态， 抓手到位或测试杯从无到有信号
    if (!z_pick_cup_state_changed && ( pick_cup_risen || (holding_cup_risen && check_holding)))
    {
        z_pick_cup_state_changed = 1;
        z_pick_cup_pos  = z_pos;
        if (srd_z.target > z_pos + z_buffer_step)
            srd_z.target = z_pos + z_buffer_step;
        srd_z.step_left = srd_z.target - z_pos;
    }

    // 去除holding_cup_fallen沿抖动干扰
    /*if (z_pick_cup_state_changed && holding_cup_risen && !holding_cup)
    {
        z_pick_cup_state_changed = 0;
        holding_cup_fallen = 0;
        srd_z.step_left = z_target - z_pos;
        if (srd_z.step_left < 0)
            srd_z.step_left = 0;
    }*/
#endif
}

// 抓杯回退时, 每一步调用一次
void Z_Rollback_Cup_Update()
{
    Z_Move_To_Update();

#if USE_PICK_CUP_SENSOR
    // 机械手抓杯传感器状态为0时, 停止电机
    if (!z_pick_cup_state_changed && pick_cup_fallen)
    {
        z_pick_cup_state_changed = 1;
        srd_z.step_left = PICK_CUP_ROLLBACK_STEP;
    }
#endif
}

////////////////////////////////////////////////////////////////////////////////
// Y电机指令

// Y电机更新定时器
void Y_Set_Timer(ushort tick)
{
    tick = tick > MAX_C ? MAX_C : tick;
    TIMER_CAR(TIMER0) = tick * 2;
    TIMER_CH0CV(TIMER0) = tick;
}

// Y电机停止定时器
void Y_Stop_Timer()
{
    timer_disable(TIMER0);
}

// 启动Y电机
void Start_Y_Motor()
{
    ro_word.y_distance = 0;
    IO_Y_DIR(srd_y.dir);
    IO_Y_EN(1);
    IO_Y_LOCK(0);
    Y_Set_Timer(FREQ_TABLE(srd_y.speed_index));
    timer_enable(TIMER0);
}

// 停止Y电机
void Stop_Y_Motor()
{
    IO_Y_LOCK(1);
    if (srd_y.dir == CCW)
        ro_word.y_distance = -srd_y.step_count;
    else
        ro_word.y_distance = srd_y.step_count;
    ro_word.y_duration = (SystemTick() - srd_y.start_time)/1000;
    srd_y.need_cleanup = 0;
    srd_y.running = 0;
}

// 执行复位指令时, 每一步调用一次
void Y_Reset_Update()
{
    // 判定原点状态改变
    if (Y_AT_HOME != srd_y.home_state)
    {
        srd_y.home_state = !srd_y.home_state;
        srd_y.hit_home = 1;
        ro_word.y_pos = 0;
        srd_y.step_left = Y_RESET_BUFFER_STEP;
    }
    else
        ro_word.y_pos += srd_y.step_offset;
}

// 复位指令结束时调用
void Y_Reset_Cleanup()
{
    if (!srd_y.hit_home)
        server_status = XY_STATUS_Y_HOME_SENSOR_ERROR;

    Stop_Y_Motor();
}

// 执行移动指令时, 每一步调用一次
void Y_Move_To_Update()
{
    // 判定原点状态改变
    if (Y_AT_HOME != srd_y.home_state)
    {
        srd_y.home_state = Y_AT_HOME;
        srd_y.hit_home = 1;
        ro_word.y_pos = 0;
        if (!srd_y.ignore_step_left)
        {
            srd_y.step_left = labs(srd_y.target);
        }
    }
    else
        ro_word.y_pos += srd_y.step_offset;
}

// 移动指令结束时调用
void Y_Move_To_Cleanup()
{
    Stop_Y_Motor();
}

////////////////////////////////////////////////////////////////////////////////

void Y_Init_Reset()
{
    srd_y.init = Start_Y_Motor;
    srd_y.start_timer = Y_Set_Timer;
    srd_y.stop_timer = Y_Stop_Timer;
    srd_y.update = Y_Reset_Update;
    srd_y.cleanup = Y_Reset_Cleanup;
    srd_y.home_state = Y_AT_HOME;
}

void Y_Init_Move(bit ignore_step_left)
{
    srd_y.init = Start_Y_Motor;
    srd_y.start_timer = Y_Set_Timer;
    srd_y.stop_timer = Y_Stop_Timer;
    srd_y.update = Y_Move_To_Update;
    srd_y.cleanup = Y_Move_To_Cleanup;
    srd_y.home_state = Y_AT_HOME;
    srd_y.ignore_step_left = ignore_step_left;
    srd_y.coder_self = 1;
    srd_y.micro = 16;
}

void Z_Init_Reset()
{
    srd_z.init = Start_Z_Motor;
    srd_z.start_timer = Z_Set_Timer;
    srd_z.stop_timer = Z_Stop_Timer;
    srd_z.update = Z_Reset_Update;
    srd_z.cleanup = Z_Reset_Cleanup;
    srd_z.home_state = Z_AT_HOME;
    srd_z.coder_self = 1;
    srd_z.micro = 16;
}

void Z_Init_Move(bit ignore_step_left)
{
    srd_z.init = Start_Z_Motor;
    srd_z.start_timer = Z_Set_Timer;
    srd_z.stop_timer = Z_Stop_Timer;
    srd_z.update = Z_Move_To_Update;
    srd_z.cleanup = Z_Move_To_Cleanup;
    srd_z.home_state = Z_AT_HOME;
    srd_z.ignore_step_left = ignore_step_left;
    srd_z.coder_self = 1;
    srd_z.micro = 16;
}

// 移动到指定坐标, 成功返回1
bit Do_Move_XY_To(long x, long y, ushort xspeed, ushort yspeed)
{
    Y_Init_Move(0);
    srd_y.target = y;
    speed_cntr_Move(&srd_y, y - ro_word.y_pos, yspeed);
    while(srd_y.running)
        Process_Main_Loop();

    return server_status == STATUS_IDLE;
}

bit Do_Z_Reset()
{
    // Z已经在零位, 动作完成
    if (Z_AT_HOME)
        return server_status == STATUS_IDLE;

    Z_Init_Reset();
    srd_z.target = srd_z.home_state ? Z_RESET_BUFFER_STEP : -Z_RESET_BUFFER_STEP;
    speed_cntr_Move(&srd_z, srd_z.home_state ? rw_word.z_max_distance : -rw_word.z_max_distance, rw_word.z_max_speed);
    while(srd_z.running)
        Process_Main_Loop();

    if(!srd_z.hit_home)
        server_status = XY_STATUS_Z_HOME_SENSOR_ERROR;
    return server_status == STATUS_IDLE;
}

// 移动到指定坐标, 成功返回1
bit Do_Move_Z_To(long z, ushort zspeed)
{
    Z_Init_Move(0);
    srd_z.target = z;
    speed_cntr_Move(&srd_z, z - ro_word.z_pos, zspeed);
    while(srd_z.running)
        Process_Main_Loop();

    return server_status == STATUS_IDLE;
}

////////////////////////////////////////////////////////////////////////////////
//debug
#ifdef PRODUCT_DEBUG
// 移动到指定坐标, 成功返回1
bit Do_Move_Y_To(long y,ushort yspeed)
{
    Y_Init_Move(0);
    srd_y.target = y;
    speed_cntr_Move(&srd_y, y - ro_word.y_pos, yspeed);
    while(srd_y.running)
        Process_Main_Loop();
    return server_status == STATUS_IDLE;
}
#endif
////////////////////////////////////////////////////////////////////////////////

bit Move_XY_To(long x, long y, ushort xspeed, ushort yspeed)
{
    return Do_Move_XY_To(x, y, xspeed, yspeed);
}

////////////////////////////////////////////////////////////////////////////////

// 将Z移动到指定位置
bit Move_XYZ_To()
{
    // 所有Z回零: 如果XY不在目标位置或者需要探测液面
    if (rw_word.y_target != ro_word.y_pos)
    {
        if (!Do_Z_Reset())
            return 0;
    }

    // XY移动到拾取/释放测试杯位置
    if (!Move_XY_To(rw_word.x_target,
                    rw_word.y_target,
                    rw_word.x_max_speed,
                    rw_word.y_max_speed))
        return 0;

#ifndef IGNORE_Z
    if(!Do_Move_Z_To(rw_word.z_target,rw_word.z_max_speed))
        return 0;
#endif//IGNORE_Z

    return 1;
}

////////////////////////////////////////////////////////////////////////////////

void Do_Action_Stop()
{
    // 停止Y电机
    if (srd_y.running)
    {
        Y_Stop_Timer();
        Stop_Y_Motor();
    }
    // 停止Z电机
    if (srd_z.running)
    {
        Z_Stop_Timer();
        Stop_Z_Motor();
    }

    server_status = STATUS_IDLE;
}

void Do_Action_Reset()
{
    // z复位
    if (!Do_Z_Reset())
    {
        return;
    }

    // 复位Y轴
    Y_Init_Reset();
    srd_y.target = srd_y.home_state ? Y_RESET_BUFFER_STEP : -Y_RESET_BUFFER_STEP;
    speed_cntr_Move(&srd_y, srd_y.home_state ? rw_word.y_max_distance : -rw_word.y_max_distance,
                    rw_word.y_max_speed < Y_MAX_RESET_SPEED ? rw_word.y_max_speed : Y_MAX_RESET_SPEED);

    // 等待Y轴完成
    while(srd_y.running)
        Process_Main_Loop();

    // 判断Y复位状态
    if (server_status != STATUS_IDLE)
        return;

    server_status = STATUS_IDLE;
}

void Do_Action_Home_YX()
{
    // z复位
    if (!Do_Z_Reset())
    {
        return;
    }

    server_status = STATUS_IDLE;

    Update_IO();
    if(!ro_bit.y_at_home)
    {
        // 复位Y轴
        Y_Init_Reset();
        srd_y.target = srd_y.home_state ? Y_RESET_BUFFER_STEP : -Y_RESET_BUFFER_STEP;
        speed_cntr_Move(&srd_y, srd_y.home_state ? rw_word.y_max_distance : -rw_word.y_max_distance, rw_word.y_max_speed);

        // 等待Y轴完成
        while(srd_y.running)
            Process_Main_Loop();

        // 判断Y复位状态
        if (server_status != STATUS_IDLE)
            return;
    }

    Update_IO();

    server_status = STATUS_IDLE;
}
// 抓杯时多走的相对位移, 步数
#define PICKUP_CUP_EXTRA_STEP   ((short)(PICKUP_CUP_EXTRA_OFFSET * SPR / Y_PERIMETER))

// 当USE_PICK_CUP_SENSOR = 0时, 该函数不会被调用, 之所以存在,
// 是因为当USE_PICK_CUP_SENSOR = 1时picker.uvproj中<OverlayString>设置中用到该函数
void Do_Action_Rollback_Cup(long target)
{
#if !USE_PICK_CUP_SENSOR
    // 没有任何用途, 只是为了消除编译警告
    // target = Z_Rollback_Cup_Update;
#else
    z_pick_cup_state_changed = 0;

    Z_Init_Move(0);
    srd_z.update = Z_Rollback_Cup_Update;
    srd_z.target = target;
    srd_z.first_stage_step = 0;
    srd_z.second_stage_step = 0;

    speed_cntr_Move(&srd_z,
                    target - ro_word.z_pos,
                    rw_word.z_max_speed);

    while(srd_z.running)
        Process_Main_Loop();
#endif
}

bit Do_Action_Z_Pick_Cup(long target)
{
    ro_word.z_pick_cup_pos = 0;
    z_pick_cup_state_changed = 0;
    z_buffer_step = Z_PROBE_BUFFER_STEP;
    check_holding = 0;
    Z_Init_Move(0);
    srd_z.update = Z_Pick_Cup_Update;
    srd_z.target = target;

    // 打开抓手
    Do_Action_Open_Picker();

    speed_cntr_Move(&srd_z,
                    target - ro_word.z_pos,
                    rw_word.z_max_speed);

    while(srd_z.running)
    {
        Process_Main_Loop();
    }

    // 关闭抓手
    Do_Action_Close_Picker();
#if CHECK_PICK_CUP_SENSOR
    if (!IO_PICK_CUP)
     {
        server_status = XY_STATUS_PICKUP_CUP_SENSOR_ERROR;
        return 0;
    }
#endif

#if USE_PICK_CUP_SENSOR
    if (z_pick_cup_state_changed)
    {
    }
    else
    {
        server_status = XY_STATUS_PICKUP_CUP_SENSOR_ERROR;
        //return 0;
    }
#else
    // 为了消除编译警告, 下面函数没有任何动作
    Do_Action_Rollback_Cup(target);
#endif

    return 1;
}

// 机械手释放测试杯
bit Do_Action_Z_Release_Cup(long target)
{
    ro_word.z_pick_cup_pos = 0;
    z_pick_cup_state_changed = 0;
    z_buffer_step = Z_RELEASE_BUFFER_STEP;
    check_holding = 0;

    Z_Init_Move(0);
    srd_z.update = Z_Pick_Cup_Update;
    srd_z.target = target;

    speed_cntr_Move(&srd_z,
                    target - ro_word.z_pos,
                    rw_word.z_max_speed);

    while(srd_z.running)
        Process_Main_Loop();

#if USE_PICK_CUP_SENSOR && CHECK_RELEASE_CUP_STATE
    if(flags & RELEASE_CUP_SENSOR_MASK)
    {
        // 忽略 PICK CUP SENSOR ERROR
    }
    else
    {
        // 抓杯命令检测到抓杯传感器后, 往回走一些步数
        if (z_pick_cup_state_changed)
        {
            /* target = z_pos - (Z_PROBE_BUFFER_STEP * 2 + PICK_CUP_ROLLBACK_STEP);
            if (target != z_pos && pick_cup_risen)
                Do_Action_Rollback_Cup(target);*/
        }
        else
        {
            server_status = XY_STATUS_RELEASE_CUP_ERROR;
            return 0;
        }
    }
#endif

    // 打开抓手
    Do_Action_Open_Picker();

    return 1;
}

// 探测表面位置
void Do_Action_Probe_Level()
{
#if USE_PICK_CUP_SENSOR
    long target;
#endif

    // 所有Z回零
    if (!Do_Z_Reset())
        return;

    // XY移动到拾取/释放测试杯位置
    if (!Move_XY_To(rw_word.x_target,
                    rw_word.y_target,
                    rw_word.x_max_speed,
                    rw_word.y_max_speed))
        return;

    ro_word.z_pick_cup_pos = 0;
    z_pick_cup_state_changed = 0;
    z_buffer_step = Z_PROBE_BUFFER_STEP;
    check_holding = 0;

    Z_Init_Move(0);
    srd_z.update = Z_Pick_Cup_Update;
    srd_z.target = rw_word.z_target;

    speed_cntr_Move(&srd_z,
                    rw_word.z_target - ro_word.z_pos,
                    rw_word.z_max_speed);

    while(srd_z.running)
        Process_Main_Loop();

#if USE_PICK_CUP_SENSOR
    // 抓杯命令检测到抓杯传感器后, 往回走一些步数
    if (z_pick_cup_state_changed)
    {
        ro_word.target = ro_word.z_pos - (Z_PROBE_BUFFER_STEP * 2 + PICK_CUP_ROLLBACK_STEP);

        if (target != z_pos && pick_cup_risen)
            Do_Action_Rollback_Cup(target);
    }
    else
    {
        server_status = XY_STATUS_PICKUP_CUP_ERROR;
    }
#endif
}

// 移动到xyz位置，然后拾取(pick=1)/释放(pick=0)测试杯
void Do_Action_Pick_Cup(bit pick)
{
    // 所有Z回零
    if (!Do_Z_Reset())
        return;

    open_triggered = 0;
    if(pick)
        Do_Action_Open_Picker_No_Wait();

    // XY移动到拾取/释放测试杯位置
    if (!Move_XY_To(rw_word.x_target,
                    rw_word.y_target,
                    rw_word.x_max_speed,
                    rw_word.y_max_speed))
        return;

#ifndef IGNORE_Z

    if (pick)
    {
        Do_Action_Z_Pick_Cup(rw_word.z_target);
    }
    else
    {
        Do_Action_Z_Release_Cup(rw_word.z_target);
    }

#endif// IGNORE_Z

#if RELEASE_CUP_DELAY
    // 放杯时, 延迟一会儿再平移
    if (!pick)
        Delay_Tick_Event(US_TO_TICK_UL(RELEASE_CUP_DELAY));
#endif// PICKUP_CUP_DELAY

#if PICKUP_CUP_DELAY
    // 抓杯时, 延迟一会儿再上移
    if (pick)
        Delay_Tick_Event(US_TO_TICK_UL(PICKUP_CUP_DELAY));
#endif// PICKUP_CUP_DELAY

#ifndef IGNORE_Z
    Z_Init_Reset();
    srd_z.target = srd_z.home_state ? Z_RESET_BUFFER_STEP : -Z_RESET_BUFFER_STEP;
    speed_cntr_Move(&srd_z, srd_z.home_state ? rw_word.z_max_distance : -rw_word.z_max_distance, rw_word.z_max_speed);
    while(srd_z.running)
    {
        Process_Main_Loop();
    }

    if(!srd_z.hit_home)
    {
        if(server_status == STATUS_IDLE)
            server_status = XY_STATUS_Z_HOME_SENSOR_ERROR;
        return;
    }

    // 放杯完成后，关闭电磁铁
    if(!pick)
    {
        Do_Action_Close_Picker();
        Delay_Tick_Event(US_TO_TICK_UL(PICKUP_CUP_DELAY));
    }

    // 检查HOLDING_CUP状态
    Update_IO();
    // 拾取/释放测试杯时, 如果机械手没有/有测试杯, 报错
    if (pick)
    {
        if(!ro_bit.holding_cup)
        {
            if(server_status == STATUS_IDLE)
                server_status = XY_STATUS_PICKUP_CUP_ERROR;
        }
    }
    else
    {

        if(ro_bit.holding_cup)
        {
            if(server_status == STATUS_IDLE)
                server_status = XY_STATUS_RELEASE_CUP_ERROR;
        }
    }

#endif// IGNORE_Z
}

#ifdef PRODUCT_DEBUG

// 从里侧孔抓杯到外侧，依次抓杯放杯
void Do_Pick_and_Release_Cup(unsigned char from,unsigned char n)
{
    char i;
    if(from > 10 || from < 1)
        return;

    rw_word.y_target = 0;
    Do_Action_Pick_Cup(1);
    rw_word.y_target = 0;// - 10;
    Do_Action_Pick_Cup(0);
    while(1)
    {
        rw_word.y_target = 0;// - 10;
        for(i = from; i < (from + n);++i)
        {
            Do_Action_Pick_Cup(1);
            rw_word.y_target = Y_STEPS_BY_ONE_HOLE * i - 10;
            Do_Action_Pick_Cup(0);
        }

        for(i = from + n - 2; i >= from - 1;--i)
        {
            Do_Action_Pick_Cup(1);
            rw_word.y_target = Y_STEPS_BY_ONE_HOLE * i - 10;
            Do_Action_Pick_Cup(0);
        }
    }
}

#endif


////////////////////////////////////////////////////////////////////////////////
// M电机指令(M 电机用于扩展功能)

// M电机更新定时器,频率输出
void M_Set_Timer(ushort tick)
{
    tick = tick > MAX_C ? MAX_C : tick;
    TIMER_CAR(TIMER1) = tick*2;
    TIMER_CH1CV(TIMER1) = tick;
}

// M电机停止定时器
void M_Stop_Timer()
{
    timer_disable(TIMER1);
}

// 启动M电机
void Start_M_Motor()
{
    ro_word.m_distance = 0;

    IO_M_DIR(srd_m.dir);
    IO_M_EN(1);
    IO_M_LOCK(0);

    M_Set_Timer(FREQ_TABLE(srd_m.speed_index));
    timer_enable(TIMER1);
}

// 停止M电机
void Stop_M_Motor()
{
    IO_M_LOCK(1);  // 锁紧电机
    if (srd_m.dir == CCW)
        ro_word.m_distance = -srd_m.step_count;
    else
        ro_word.m_distance = srd_m.step_count;
    ro_word.m_duration = (SystemTick() - srd_m.start_time)/1000;
    srd_m.need_cleanup = 0;
    srd_m.running = 0;
}

// 执行复位指令时, 每一步调用一次
void M_Reset_Update()
{
    // 判定原点状态改变
    if (M_AT_HOME != srd_m.home_state)
    {
        srd_m.home_state = !srd_m.home_state;
        srd_m.hit_home = 1;
        ro_word.m_pos = 0;
        srd_m.step_left = Z_RESET_BUFFER_STEP;
    }
    else
        ro_word.m_pos += srd_m.step_offset;
}

// 复位指令结束时调用
void M_Reset_Cleanup()
{
    if (!srd_m.hit_home)
        server_status = STATUS_BUSY; // M电机复位失败状态

    Stop_M_Motor();
}

// 执行移动指令时, 每一步调用一次
void M_Move_To_Update()
{
    // 当剩余步数少于一定值时，启用霍尔传感器检测
    if (srd_m.step_left <= M_HALL_DETECTION_THRESHOLD && !hall_detection_enabled)
    {
        hall_detection_enabled = 1;
    }

    // 只有在启用霍尔检测时才处理霍尔传感器信号
    if (hall_detection_enabled)
    {
        // 判定原点状态改变
        if (M_AT_HOME != srd_m.home_state)
        {
            srd_m.home_state = !srd_m.home_state;
            srd_m.hit_home = 1;
            ro_word.m_pos = 0;
            if (!srd_m.ignore_step_left)
            {
                srd_m.step_left = labs(srd_m.target);
            }
        }
        else
            ro_word.m_pos += srd_m.step_offset;
    }
    else
    {
        // 运行时忽略霍尔传感器，只更新位置
        ro_word.m_pos += srd_m.step_offset;
    }
}

// 移动指令结束时调用
void M_Move_To_Cleanup()
{
    Stop_M_Motor();
}

void M_Init_Reset()
{
    srd_m.init = Start_M_Motor;
    srd_m.start_timer = M_Set_Timer;
    srd_m.stop_timer = M_Stop_Timer;
    srd_m.update = M_Reset_Update;
    srd_m.cleanup = M_Reset_Cleanup;
    srd_m.home_state = M_AT_HOME;
    srd_y.coder_self = 1;
    srd_y.micro = 16;
}

void M_Init_Move(bit ignore_step_left)
{
    srd_m.init = Start_M_Motor;
    srd_m.start_timer = M_Set_Timer;
    srd_m.stop_timer = M_Stop_Timer;
    srd_m.update = M_Move_To_Update;
    srd_m.cleanup = M_Move_To_Cleanup;
    srd_m.home_state = M_AT_HOME;
    srd_m.ignore_step_left = ignore_step_left;
    srd_m.coder_self = 1;
    srd_m.micro = 16;

    // 重置霍尔检测标志，确保每次启动时都从忽略状态开始
    hall_detection_enabled = 0;
}


bit Do_Action_Magent_Revolve(int speed)
{
    int time_seconds = Extract_Time_From_XTarget(rw_word.x_target);
    long steps = Calculate_Steps_From_Time_FreqPL(time_seconds,speed);

    M_Init_Move(0);
    speed_cntr_Move(&srd_m,steps,speed); // x_target高16位为时间，单位: s
    while(srd_m.running)
        Main_Loop();

    return 1;
}

void Do_Action_Disable_Motor()
{
    IO_X_EN(0);
    IO_Y_EN(0);
    IO_Z_EN(0);
    IO_M_EN(0);
}

void Process_Action()
{
    switch (rw_word.action)
    {
    case XY_ACTION_STOP:
        Do_Action_Stop();
        ps_server_busy = 0;
        break;

    case XY_ACTION_RESET:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Do_Action_Reset();
        ps_server_busy = 0;
        break;

    case XY_ACTION_PICK_CUP:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Do_Action_Pick_Cup(1);
        ps_server_busy = 0;
        break;

    case XY_ACTION_RELEASE_CUP:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Do_Action_Pick_Cup(0);
        ps_server_busy = 0;
        break;

    case XY_ACTION_TRANSFER_CUP:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Do_Action_Pick_Cup(1);
        if (server_status == STATUS_IDLE)
        {
            rw_word.y_target = rw_word.y_target_2;
            rw_word.z_target = rw_word.z_target_2;
            Do_Action_Pick_Cup(0);
        }
        ps_server_busy = 0;
        break;

    case XY_ACTION_MOVE_XY:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Move_XYZ_To();
        ps_server_busy = 0;
        break;
    case XY_ACTION_PROBE_LEVEL:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Do_Action_Probe_Level();
        ps_server_busy = 0;
        break;
    case XY_ACTION_DISABLE_MOTOR:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Do_Action_Disable_Motor();
        ps_server_busy = 0;
        break;
    case XY_ACTION_MOVE_XYZ_0:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Move_XYZ_To();
        ps_server_busy = 0;
        break;
    case XY_ACTION_MOVE_XYZ_1:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Move_XYZ_To();
        ps_server_busy = 0;
        break;
    case XY_ACTION_MOVE_XYZ_2:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Move_XYZ_To();
        ps_server_busy = 0;
        break;
    case XY_ACTION_HOME_YX:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Do_Action_Home_YX();
        ps_server_busy = 0;
        break;
    case XY_ACTION_OPEN_PICKER:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Do_Action_Open_Picker();
        ps_server_busy = 0;
        break;
    case XY_ACTION_CLOSE_PICKER:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Do_Action_Close_Picker();
        ps_server_busy = 0;
        break;
    case XY_ACTION_DO_MIX:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Do_Action_Magent_Revolve(rw_word.m_max_speed);
        ps_server_busy = 0;
        break;

#ifdef PRODUCT_DEBUG
    case XY_ACTION_RESET_Z:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Do_Z_Reset();
        ps_server_busy = 0;
        break;
    case XY_ACTION_MOVE_Z:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Do_Move_Z_To(rw_word.z_target,rw_word.z_max_speed);
        ps_server_busy = 0;
        break;
    case XY_ACTION_Z_PICK:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Do_Action_Z_Pick_Cup(rw_word.z_target);
        ps_server_busy = 0;
        break;
    case XY_ACTION_MOVE_X:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        ps_server_busy = 0;
        break;
    case XY_ACTION_MOVE_Y:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Do_Move_Y_To(rw_word.y_target,rw_word.y_max_speed);
        ps_server_busy = 0;
        break;
    case XY_ACTION_PICK_RELEASE_CUP:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Do_Pick_and_Release_Cup(1,9);
        ps_server_busy = 0;
        break;
#endif
    default:
        break;
    }
}
